/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package org.team3309.beta.subsystems;

import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.team3309.beta.commands.JoystickDrive;
import org.team3309.beta.commands.TeleopControl;

/**
 *
 * @author vmagro
 */
public class Drive extends Subsystem {

    private static Drive instance = null;
    
    private static JoystickDrive command = new JoystickDrive(1);
    private RobotDrive drive = null;
    private Gyro gyro = null;
    
    //<editor-fold desc="CANJaguars">
    private CANJaguar lFront;
    private CANJaguar lBack;
    private CANJaguar rFront;
    private CANJaguar rBack;
    //</editor-fold>
    
    //<editor-fold desc="CAN Initialization variables">
    private static final int MAX_CAN_ATTEMPTS = 5;
    private int canInitAttempts = 0;
    private boolean canSuccesful = false;
    //</editor-fold>
    
    /*
     * Method used to get the singleton instance of this class
     * Follows singleton pattern so as to not have multiple instances of this class and it's corresponding members
     * @return 
     */
    public static Drive getInstance() {
        if (instance == null) {
            instance = new Drive();
            // Set default command here, like this:
            // instance.setDefaultCommand(new CommandIWantToRun());
            instance.setDefaultCommand(command);
        }
        return instance;
    }

    /*
     * Initializes the subsystem
     */
    private Drive() {
        initCAN();
        drive = new RobotDrive(lFront, lBack, rFront, rBack);
        gyro = Gyro.getInstance();
    }
    
    /**
     * Attempts to initialize the CANJaguars
     * Tries MAX_CAN_ATTEMPTS to initialize the Jaguars in case of CANTimeoutException being thrown
     * This method recalls itself up to MAX_CAN_ATTEMPTS to attempt to correct temporary CAN timeouts that may occur when the robot boots
     */
    private void initCAN(){
        if(canInitAttempts < MAX_CAN_ATTEMPTS)
            try {
                lFront  = new CANJaguar(3);
                lBack   = new CANJaguar(2);
                rFront  = new CANJaguar(4);
                rBack   = new CANJaguar(5);
                canInitAttempts++;
            } catch (CANTimeoutException ctex) {
                ctex.printStackTrace();
                initCAN();
            }
        else canSuccesful = false;
    }
    
    public void mecanumDrive(double x, double y, double twist, double g){
        gyro.updateDesiredHeading(twist);
        System.out.println(twist+" "+gyro.getTwistRate());
        drive.mecanumDrive_Cartesian(-x, -y, gyro.getTwistRate(), 0);
    }
    
    public void mecanumDrive(double x, double y, double twist){
        mecanumDrive(x, y, twist, 0);
    }
}
